#ifndef __DRIVETOPATH_H
#define __DRIVETOPATH_H

#include "rclcpp/rclcpp.hpp"
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
// 2023.12.18 modify by haibo
#include <agv_msgs/msg/agv_mode.hpp>
#include <std_msgs/msg/bool.hpp>
#include "World.h"
#include <tf/tf.h>
#include <thread>
#include <mutex>
#include <dynamic_reconfigure/server.h>
#include <agv_navigate/paramsConfig.h>

using namespace std;

enum CtrlMode
{
    NoneMode = 0,
    ManualMode = 1,
    HalfAutoMode = 2,
    AutoMode = 3
};

enum RunMode
{
    CloseRun = 0,
    SafeRun = 1,
    AutoRun = 2
};

enum RunState
{
    NoneState = 0,
    StartAngleAdjust = 1,
    MoveToTarget = 2,
    FinalAngleAdjust = 3
};

struct TargetPos
{
    double dis;
    double x;
    double y;
    double theta; // angle value in degree
    TargetPos()
    {
        dis = 0;
        x = 0;
        y = 0;
        theta = 0;
    }
};

class driveToPath
{
public:
    driveToPath(rclcpp::Node &nh);
    ~driveToPath();
    bool init();

private:
    // 2023.12.18 modify by haibo
    void agvModeCallBack(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg);
    void allowMoveCallBack(const std_msgs::msg::Bool::ConstSharedPtr &msg);
    bool GetBit(int value, int index);

    std::mutex m_mutex;
    std::thread GetCurPosThread; //线程：用于获取当前位姿，并计算与目标点距离角度，更新状态
    std::thread AgvDrivingThread;//线程：用于计算速度
    static void getCurPosThreadFun(driveToPath *pThis);
    static void agvDrivingThreadFun(driveToPath *pThis);

    // void initTarget();
    void updateTarget(TargetPos fTargetPos);
    TargetPos getFinallyTargetPoint();
    //计算点到路径的最近距离与最近位置
    TargetPos PointToLine(double x, double y, CPath *pPath);
    TargetPos PointToBezier(double x, double y, CPath *pPath);
    TargetPos PointToSide(double x, double y, CPath *pPath);
    //根据相对位姿信息，实时计算速度值并发布
    void doDriving();
    //外部设置参数初始化
    void externalParamsInit();
    //变量初始化
    void variableInit();

    bool m_driveToPathInitOk = false;
    rclcpp::Node m_nh;
    // force control agv to stop topic (to twist_mux)
    ros::Publisher m_vel_pub;
    // 2023.12.18 modify by haibo
    ros::Subscriber m_agvmode_sub; 
    ros::Subscriber m_odom_sub;

    ros::Publisher m_allowMove_pub;
    ros::Subscriber m_allowMove_sub;

    int m_curCtrlMode;
    int m_lastCtrlMode;
    // bool m_isHalfAuto;    //半自动状态中
    bool m_isOnPath;    //是否已经行驶到路径上了
    bool m_isAllowMove; //是否允许移动
    int m_curState;     // 0-角度√距离√；1-角度×距离×；2-角度√距离×；3-角度×距离√；
    double m_curPosX;   //当前位置x
    double m_curPosY;   //当前位置y
    double m_curPosT;   //当前位置theta
    //用于阻塞流程，确保顺序：位置更新---目标---差值状态
    bool m_curPosIsUpdate;    // 当前位姿已经刷新
    bool m_curTargetIsUpdate; // 当前目标已经刷新
    bool m_curStateIsUpdate;  // 当前状态已经刷新
    //实时目标值与相对位置
    double m_curTargetX; //目标点x
    double m_curTargetY; //目标点y
    double m_curTargetT; //当前目标角度
    double m_endTargetT; //最终目标角度
    double m_curDis;     //距离差
    double m_curAng;     //角度差
    double m_endAng;     //最终角度差
    int m_sign;          //正负系数（用于调整旋转方向）

    double m_stopAngOut; //角度偏差值大阈值

    //外部设置参数
    int m_mode;  //0关闭，1安全运行，2自动运行
    double m_dis_max;
    double m_high_speed;
    double m_low_speed;

    /*********************
    ** Dynamic Reconfigure
    **********************/
    dynamic_reconfigure::Server<agv_navigate::paramsConfig> *dynamic_reconfigure_server;
    dynamic_reconfigure::Server<agv_navigate::paramsConfig>::CallbackType dynamic_reconfigure_callback;
    void reconfigCB(agv_navigate::paramsConfig &config, uint32_t unused_level);
};
#endif
